#include "control_base/WMJRobotControl.h"

namespace wmj
{
    /*
     * @brief 操作底盘
     *
     * @param: uint_8 是否开启小陀螺和当前x,y数据是否有效，还有是否启用电容, float x速度, float y速度,
     */
    void WMJRobotControl::setChassis(uint8_t info1, float x_speed, float y_speed)
    {
        CHMPack control_data{};
        Buffer data_to_send{};

        control_data.msg.cha_info1 = info1;
        control_data.msg.cha_x_speed_high = (uint8_t)(((int16_t)(x_speed / 3.5 * 32678)) >> 8);
        control_data.msg.cha_x_speed_low = (uint8_t)((int16_t)(x_speed / 3.5 * 32678));
        control_data.msg.cha_y_speed_high = (uint8_t)(((int16_t)(y_speed / 3.5 * 32678)) >> 8);
        control_data.msg.cha_y_speed_low = (uint8_t)((int16_t)(y_speed / 3.5 * 32678));
        control_data.msg.cha_placehoder1 = 0;
        control_data.msg.cha_placehoder2 = 0;
        control_data.msg.cha_placehoder3 = 0;

        data_to_send.push_back((uint8_t)Chassis);
        for (auto c : control_data.data)
            data_to_send.push_back(c);

        sendFrame(data_to_send);
        // canBus->sendFrame(data_to_send);
    }

    /*
     * @brief 设置速度与小陀螺状态
     *
     * @param: bool 是否开启小陀螺（1为是，0为否）,  float x速度, float y速度，
     */
    void WMJRobotControl::setTopAndSpeed(bool power, bool use_cap, float x_speed, float y_speed)
    {
        m_curTopStatus = power;
        
        // 根据bit位定义构造控制字节：
        // 使用高4位，低4位保留
        // bit0（最高位，第7位）: 是否控陀螺
        // bit1（第6位）: 是否允许x方向速度
        // bit2（第5位）: 是否允许y方向速度
        // bit3（第4位）: 是否启用电容
        // 后4位保留
        uint8_t control_byte = 0x00;
        
        // 设置是否控陀螺（bit0，第7位）
        if (power)
            control_byte |= (1 << 7);
        
        // 设置是否允许x方向速度（bit1，第6位）
        if (x_speed != 0)
            control_byte |= (1 << 6);
        
        // 设置是否允许y方向速度（bit2，第5位）
        if (y_speed != 0)
            control_byte |= (1 << 5);
            
        // 设置是否启用电容（bit3，第4位）
        if (use_cap)
            control_byte |= (1 << 4);
            
        // 调用setChassis函数，传入构造好的控制字节
        this->setChassis(control_byte, x_speed, y_speed);
    }

    void WMJRobotControl::setChassisSpeed(float x_speed, float y_speed)
    {
        uint8_t control_byte = 0x00;
        
        // 保持当前小陀螺状态
        if (m_curTopStatus)
            control_byte |= (1 << 7);
            
        // 设置x方向速度
        if (x_speed != 0)
            control_byte |= (1 << 6);
            
        // 设置y方向速度
        if (y_speed != 0)
            control_byte |= (1 << 5);
        
        this->setChassis(control_byte, x_speed, y_speed);
    }
    /*
     * @brief  小陀螺开关
     *
     * @param: bool  是否开启小陀螺（1为是，0为否）
     */
    void WMJRobotControl::top(bool power)
    {
        m_curTopStatus = power;
        
        uint8_t control_byte = 0x00;
        
        // 只设置小陀螺控制位
        if (power)
            control_byte |= (1 << 7);
            
        this->setChassis(control_byte, 0, 0);
    }
    /*
     * @brief 刹车
     */
    void WMJRobotControl::brake()
    {
        this->setChassis(0x00, 0, 0);
    }
    /*
     * @brief  查看当前速度
     *
     * 返回std::vector<float>  x_speed ,y_speed;
     */
    std::vector<float> WMJRobotControl::getSpeed()
    {
        float speed[2];
        float x_speed = 0;
        float y_speed = 0;
        Buffer data_from_read;
        data_from_read = readFrame(Chassis);

        x_speed = (int16_t)((int16_t)(data_from_read[2]) | ((int16_t)(data_from_read[1]) << 8));
        y_speed = (int16_t)((int16_t)(data_from_read[4]) | ((int16_t)(data_from_read[3]) << 8));

        speed[0] = x_speed / 32768 * 3.5;
        speed[1] = y_speed / 32768 * 3.5;
        return std::vector<float>(speed, speed + 2);
    }
    /*
     * @brief  查看是否有小陀螺
     *
     * 返回1为开启，0为未开启
     */
    bool WMJRobotControl::get_top()
    {
        int flag;
        Buffer data_from_read;
        data_from_read = readFrame(Chassis);
        flag = data_from_read[0] & 0x40;
        if (flag)
        {
            return true;
        }
        else
        {
            return false;
        }
    }
}
